import rclpy
from rclpy.node import Node
import rclpy.time
from tf2_ros import TransformListener,Buffer        #坐标监听
from tf_transformations import euler_from_quaternion#四元数欧拉函数
import math


class TFBroadcaster(Node):
    def __init__(self,name):
        super().__init__(name)
        self.buffer=Buffer()
        #         # 声明使用仿真时间参数
        # self.declare_parameter('use_sim_time', False)
        # self.set_parameters([rclpy.Parameter('use_sim_time', 
        #                    value=self.get_parameter('use_sim_time').value)])
        self.listener=TransformListener(self.buffer,self)
        self.timer_=self.create_timer(1.0,self.get_transform)
    
    def get_transform(self):
        #实时查询坐标关系

        try:
            # now=self.get_clock().now()
            result=self.buffer.lookup_transform('map','base_footprint',
                    rclpy.time.Time(seconds=0.0),rclpy.time.Duration(seconds=1.0))#
            transform=result.transform
            self.get_logger().info(f'平移：{transform.translation}')
            self.get_logger().info(f'旋转: {transform.rotation}' )
            rotation_euler=euler_from_quaternion([
                transform.rotation.x,
                transform.rotation.y,
                transform.rotation.z,
                transform.rotation.w])
            self.get_logger().info(f'旋转RPY:{rotation_euler}')

        except Exception as e:
            self.get_logger().info(f'获取坐标变换失败：{str(e)}')
        

def main(args=None):
    rclpy.init(args=args)
    node=TFBroadcaster("tf_boadcaster")
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()